import time

import rtde_receive
import rtde_control
from Hardware import FT300
from configparser import ConfigParser
from Hardware.FTur5 import Ftur5

conf = ConfigParser()
conf.read('config.ini')
robotip=conf['hardware']['robotip']
rtde_r = rtde_receive.RTDEReceiveInterface(robotip)
rtde_c=rtde_control.RTDEControlInterface(robotip)
ft= Ftur5()
time.sleep(2)




actual_q = rtde_r.getActualQ()
goal_joint0=[-2.45614463487734, -1.3294060865985315, -1.6362646261798304, -1.5686772505389612, 1.5779247283935547, 0.21282486617565155]
goal_joint1=[-2.85614463487734, -1.3294060865985315, -1.6362646261798304, -1.5686772505389612, 1.5779247283935547, 0.21282486617565155]
rtde_c.zeroFtSensor()
#rtde_c.zeroFtSensor()
print("=============================")
ft.FtZero()
count=0
rtde_c.freedriveMode()

while(True):
    #time.sleep(1)
    count+=1
    if(count==1000):
        count=0
        print(rtde_r.getActualTCPPose())
        print(ft.GetForce())
        #print(ft.GetForce())
        #time.sleep(0.5)
        #print(rtde_r.getActualQ())


force=rtde_r.getActualTCPForce()

print(actual_q)
print(force)
rtde_c.endTeachMode()

# rtde_c.moveJ(goal_joint0,1,0.1,False)
# rtde_c.moveJ(goal_joint1,0.1,0.1,False)
